function dz = dynamics_10_link(~,z,P) 
%DZ = DYNAMICS_10_LINK(T,Z,P)
% 
%FUNCTION:  This function computes the dynamics of a 10
%    link pendulum, and is designed to be called from ode45.
%    The model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
% 
%INPUTS: 
%    t = time. Dummy input for ode45. Not used.
%    z = [20 X nTime]  matrix of states
%    P = struct of parameters
%OUTPUTS: 
%    dz = [20 X nTime]  matrix of state derivatives
% 
%NOTES:
%    This file was automatically generated by writeDynamics.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
m7 = P.m(7); % Link 7 mass
m8 = P.m(8); % Link 8 mass
m9 = P.m(9); % Link 9 mass
m10 = P.m(10); % Link 10 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
l7 = P.l(7); % Link 7 length
l8 = P.l(8); % Link 8 length
l9 = P.l(9); % Link 9 length
l10 = P.l(10); % Link 10 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
I7 = P.I(7); % Link 7 moment of inertia about its center of mass
I8 = P.I(8); % Link 8 moment of inertia about its center of mass
I9 = P.I(9); % Link 9 moment of inertia about its center of mass
I10 = P.I(10); % Link 10 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint
d7 = P.d(7); % Link 7 distance between center of mass and parent joint
d8 = P.d(8); % Link 8 distance between center of mass and parent joint
d9 = P.d(9); % Link 9 distance between center of mass and parent joint
d10 = P.d(10); % Link 10 distance between center of mass and parent joint

nTime = size(z,2);
dz = zeros(size(z)); 
M = zeros(10,10,nTime);
f = zeros(10,nTime);

th1 = z(1,:); 
th2 = z(2,:); 
th3 = z(3,:); 
th4 = z(4,:); 
th5 = z(5,:); 
th6 = z(6,:); 
th7 = z(7,:); 
th8 = z(8,:); 
th9 = z(9,:); 
th10 = z(10,:); 

dth1 = z(11,:); 
dth2 = z(12,:); 
dth3 = z(13,:); 
dth4 = z(14,:); 
dth5 = z(15,:); 
dth6 = z(16,:); 
dth7 = z(17,:); 
dth8 = z(18,:); 
dth9 = z(19,:); 
dth10 = z(20,:); 

dz(1,:) = dth1; 
dz(2,:) = dth2; 
dz(3,:) = dth3; 
dz(4,:) = dth4; 
dz(5,:) = dth5; 
dz(6,:) = dth6; 
dz(7,:) = dth7; 
dz(8,:) = dth8; 
dz(9,:) = dth9; 
dz(10,:) = dth10; 

M(1,1,:) = - I1 - d1.^2.*m1 - l1.^2.*m2 - l1.^2.*m3 - l1.^2.*m4 - l1.^2.*m5 - l1.^2.*m6 - l1.^2.*m7 - l1.^2.*m8 - l1.^2.*m9 - l1.^2.*m10;
M(1,2,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6 + l2.*m7 + l2.*m8 + l2.*m9 + l2.*m10);
M(1,3,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6 + l3.*m7 + l3.*m8 + l3.*m9 + l3.*m10);
M(1,4,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(1,5,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(1,6,:) = -l1.*cos(th1 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(1,7,:) = -l1.*cos(th1 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(1,8,:) = -l1.*cos(th1 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(1,9,:) = -l1.*cos(th1 - th9).*(d9.*m9 + l9.*m10);
M(1,10,:) = -d10.*l1.*m10.*cos(th1 - th10);
M(2,1,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6 + l2.*m7 + l2.*m8 + l2.*m9 + l2.*m10);
M(2,2,:) = - I2 - d2.^2.*m2 - l2.^2.*m3 - l2.^2.*m4 - l2.^2.*m5 - l2.^2.*m6 - l2.^2.*m7 - l2.^2.*m8 - l2.^2.*m9 - l2.^2.*m10;
M(2,3,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6 + l3.*m7 + l3.*m8 + l3.*m9 + l3.*m10);
M(2,4,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(2,5,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(2,6,:) = -l2.*cos(th2 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(2,7,:) = -l2.*cos(th2 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(2,8,:) = -l2.*cos(th2 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(2,9,:) = -l2.*cos(th2 - th9).*(d9.*m9 + l9.*m10);
M(2,10,:) = -d10.*l2.*m10.*cos(th2 - th10);
M(3,1,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6 + l3.*m7 + l3.*m8 + l3.*m9 + l3.*m10);
M(3,2,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6 + l3.*m7 + l3.*m8 + l3.*m9 + l3.*m10);
M(3,3,:) = - I3 - d3.^2.*m3 - l3.^2.*m4 - l3.^2.*m5 - l3.^2.*m6 - l3.^2.*m7 - l3.^2.*m8 - l3.^2.*m9 - l3.^2.*m10;
M(3,4,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(3,5,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(3,6,:) = -l3.*cos(th3 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(3,7,:) = -l3.*cos(th3 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(3,8,:) = -l3.*cos(th3 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(3,9,:) = -l3.*cos(th3 - th9).*(d9.*m9 + l9.*m10);
M(3,10,:) = -d10.*l3.*m10.*cos(th3 - th10);
M(4,1,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(4,2,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(4,3,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6 + l4.*m7 + l4.*m8 + l4.*m9 + l4.*m10);
M(4,4,:) = - I4 - d4.^2.*m4 - l4.^2.*m5 - l4.^2.*m6 - l4.^2.*m7 - l4.^2.*m8 - l4.^2.*m9 - l4.^2.*m10;
M(4,5,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(4,6,:) = -l4.*cos(th4 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(4,7,:) = -l4.*cos(th4 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(4,8,:) = -l4.*cos(th4 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(4,9,:) = -l4.*cos(th4 - th9).*(d9.*m9 + l9.*m10);
M(4,10,:) = -d10.*l4.*m10.*cos(th4 - th10);
M(5,1,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(5,2,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(5,3,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(5,4,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6 + l5.*m7 + l5.*m8 + l5.*m9 + l5.*m10);
M(5,5,:) = - I5 - d5.^2.*m5 - l5.^2.*m6 - l5.^2.*m7 - l5.^2.*m8 - l5.^2.*m9 - l5.^2.*m10;
M(5,6,:) = -l5.*cos(th5 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(5,7,:) = -l5.*cos(th5 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(5,8,:) = -l5.*cos(th5 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(5,9,:) = -l5.*cos(th5 - th9).*(d9.*m9 + l9.*m10);
M(5,10,:) = -d10.*l5.*m10.*cos(th5 - th10);
M(6,1,:) = -l1.*cos(th1 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(6,2,:) = -l2.*cos(th2 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(6,3,:) = -l3.*cos(th3 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(6,4,:) = -l4.*cos(th4 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(6,5,:) = -l5.*cos(th5 - th6).*(d6.*m6 + l6.*m7 + l6.*m8 + l6.*m9 + l6.*m10);
M(6,6,:) = - I6 - d6.^2.*m6 - l6.^2.*m7 - l6.^2.*m8 - l6.^2.*m9 - l6.^2.*m10;
M(6,7,:) = -l6.*cos(th6 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(6,8,:) = -l6.*cos(th6 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(6,9,:) = -l6.*cos(th6 - th9).*(d9.*m9 + l9.*m10);
M(6,10,:) = -d10.*l6.*m10.*cos(th6 - th10);
M(7,1,:) = -l1.*cos(th1 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,2,:) = -l2.*cos(th2 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,3,:) = -l3.*cos(th3 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,4,:) = -l4.*cos(th4 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,5,:) = -l5.*cos(th5 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,6,:) = -l6.*cos(th6 - th7).*(d7.*m7 + l7.*m8 + l7.*m9 + l7.*m10);
M(7,7,:) = - I7 - d7.^2.*m7 - l7.^2.*m8 - l7.^2.*m9 - l7.^2.*m10;
M(7,8,:) = -l7.*cos(th7 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(7,9,:) = -l7.*cos(th7 - th9).*(d9.*m9 + l9.*m10);
M(7,10,:) = -d10.*l7.*m10.*cos(th7 - th10);
M(8,1,:) = -l1.*cos(th1 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,2,:) = -l2.*cos(th2 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,3,:) = -l3.*cos(th3 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,4,:) = -l4.*cos(th4 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,5,:) = -l5.*cos(th5 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,6,:) = -l6.*cos(th6 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,7,:) = -l7.*cos(th7 - th8).*(d8.*m8 + l8.*m9 + l8.*m10);
M(8,8,:) = - I8 - d8.^2.*m8 - l8.^2.*m9 - l8.^2.*m10;
M(8,9,:) = -l8.*cos(th8 - th9).*(d9.*m9 + l9.*m10);
M(8,10,:) = -d10.*l8.*m10.*cos(th8 - th10);
M(9,1,:) = -l1.*cos(th1 - th9).*(d9.*m9 + l9.*m10);
M(9,2,:) = -l2.*cos(th2 - th9).*(d9.*m9 + l9.*m10);
M(9,3,:) = -l3.*cos(th3 - th9).*(d9.*m9 + l9.*m10);
M(9,4,:) = -l4.*cos(th4 - th9).*(d9.*m9 + l9.*m10);
M(9,5,:) = -l5.*cos(th5 - th9).*(d9.*m9 + l9.*m10);
M(9,6,:) = -l6.*cos(th6 - th9).*(d9.*m9 + l9.*m10);
M(9,7,:) = -l7.*cos(th7 - th9).*(d9.*m9 + l9.*m10);
M(9,8,:) = -l8.*cos(th8 - th9).*(d9.*m9 + l9.*m10);
M(9,9,:) = - I9 - d9.^2.*m9 - l9.^2.*m10;
M(9,10,:) = -d10.*l9.*m10.*cos(th9 - th10);
M(10,1,:) = -d10.*l1.*m10.*cos(th1 - th10);
M(10,2,:) = -d10.*l2.*m10.*cos(th2 - th10);
M(10,3,:) = -d10.*l3.*m10.*cos(th3 - th10);
M(10,4,:) = -d10.*l4.*m10.*cos(th4 - th10);
M(10,5,:) = -d10.*l5.*m10.*cos(th5 - th10);
M(10,6,:) = -d10.*l6.*m10.*cos(th6 - th10);
M(10,7,:) = -d10.*l7.*m10.*cos(th7 - th10);
M(10,8,:) = -d10.*l8.*m10.*cos(th8 - th10);
M(10,9,:) = -d10.*l9.*m10.*cos(th9 - th10);
M(10,10,:) = - I10 - d10.^2.*m10;

f(1,:) = - d1.*g.*m1.*cos(th1) - g.*l1.*m2.*cos(th1) - g.*l1.*m3.*cos(th1) - g.*l1.*m4.*cos(th1) - g.*l1.*m5.*cos(th1) - g.*l1.*m6.*cos(th1) - g.*l1.*m7.*cos(th1) - g.*l1.*m8.*cos(th1) - g.*l1.*m9.*cos(th1) - g.*l1.*m10.*cos(th1) - d2.*dth2.^2.*l1.*m2.*sin(th1 - th2) - d3.*dth3.^2.*l1.*m3.*sin(th1 - th3) - d4.*dth4.^2.*l1.*m4.*sin(th1 - th4) - d5.*dth5.^2.*l1.*m5.*sin(th1 - th5) - d6.*dth6.^2.*l1.*m6.*sin(th1 - th6) - d7.*dth7.^2.*l1.*m7.*sin(th1 - th7) - d8.*dth8.^2.*l1.*m8.*sin(th1 - th8) - d9.*dth9.^2.*l1.*m9.*sin(th1 - th9) - d10.*dth10.^2.*l1.*m10.*sin(th1 - th10) - dth2.^2.*l1.*l2.*m3.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m4.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m5.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m6.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m7.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m4.*sin(th1 - th3) - dth2.^2.*l1.*l2.*m8.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m5.*sin(th1 - th3) - dth2.^2.*l1.*l2.*m9.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m6.*sin(th1 - th3) - dth2.^2.*l1.*l2.*m10.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m7.*sin(th1 - th3) - dth3.^2.*l1.*l3.*m8.*sin(th1 - th3) - dth4.^2.*l1.*l4.*m5.*sin(th1 - th4) - dth3.^2.*l1.*l3.*m9.*sin(th1 - th3) - dth4.^2.*l1.*l4.*m6.*sin(th1 - th4) - dth3.^2.*l1.*l3.*m10.*sin(th1 - th3) - dth4.^2.*l1.*l4.*m7.*sin(th1 - th4) - dth4.^2.*l1.*l4.*m8.*sin(th1 - th4) - dth4.^2.*l1.*l4.*m9.*sin(th1 - th4) - dth5.^2.*l1.*l5.*m6.*sin(th1 - th5) - dth4.^2.*l1.*l4.*m10.*sin(th1 - th4) - dth5.^2.*l1.*l5.*m7.*sin(th1 - th5) - dth5.^2.*l1.*l5.*m8.*sin(th1 - th5) - dth5.^2.*l1.*l5.*m9.*sin(th1 - th5) - dth5.^2.*l1.*l5.*m10.*sin(th1 - th5) - dth6.^2.*l1.*l6.*m7.*sin(th1 - th6) - dth6.^2.*l1.*l6.*m8.*sin(th1 - th6) - dth6.^2.*l1.*l6.*m9.*sin(th1 - th6) - dth6.^2.*l1.*l6.*m10.*sin(th1 - th6) - dth7.^2.*l1.*l7.*m8.*sin(th1 - th7) - dth7.^2.*l1.*l7.*m9.*sin(th1 - th7) - dth7.^2.*l1.*l7.*m10.*sin(th1 - th7) - dth8.^2.*l1.*l8.*m9.*sin(th1 - th8) - dth8.^2.*l1.*l8.*m10.*sin(th1 - th8) - dth9.^2.*l1.*l9.*m10.*sin(th1 - th9);
f(2,:) = d2.*dth1.^2.*l1.*m2.*sin(th1 - th2) - g.*l2.*m3.*cos(th2) - g.*l2.*m4.*cos(th2) - g.*l2.*m5.*cos(th2) - g.*l2.*m6.*cos(th2) - g.*l2.*m7.*cos(th2) - g.*l2.*m8.*cos(th2) - g.*l2.*m9.*cos(th2) - g.*l2.*m10.*cos(th2) - d2.*g.*m2.*cos(th2) - d3.*dth3.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l2.*m4.*sin(th2 - th4) - d5.*dth5.^2.*l2.*m5.*sin(th2 - th5) - d6.*dth6.^2.*l2.*m6.*sin(th2 - th6) - d7.*dth7.^2.*l2.*m7.*sin(th2 - th7) - d8.*dth8.^2.*l2.*m8.*sin(th2 - th8) - d9.*dth9.^2.*l2.*m9.*sin(th2 - th9) - d10.*dth10.^2.*l2.*m10.*sin(th2 - th10) + dth1.^2.*l1.*l2.*m3.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m4.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m5.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m6.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m7.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m8.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m9.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m10.*sin(th1 - th2) - dth3.^2.*l2.*l3.*m4.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m5.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m6.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m7.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m8.*sin(th2 - th3) - dth4.^2.*l2.*l4.*m5.*sin(th2 - th4) - dth3.^2.*l2.*l3.*m9.*sin(th2 - th3) - dth4.^2.*l2.*l4.*m6.*sin(th2 - th4) - dth3.^2.*l2.*l3.*m10.*sin(th2 - th3) - dth4.^2.*l2.*l4.*m7.*sin(th2 - th4) - dth4.^2.*l2.*l4.*m8.*sin(th2 - th4) - dth4.^2.*l2.*l4.*m9.*sin(th2 - th4) - dth5.^2.*l2.*l5.*m6.*sin(th2 - th5) - dth4.^2.*l2.*l4.*m10.*sin(th2 - th4) - dth5.^2.*l2.*l5.*m7.*sin(th2 - th5) - dth5.^2.*l2.*l5.*m8.*sin(th2 - th5) - dth5.^2.*l2.*l5.*m9.*sin(th2 - th5) - dth5.^2.*l2.*l5.*m10.*sin(th2 - th5) - dth6.^2.*l2.*l6.*m7.*sin(th2 - th6) - dth6.^2.*l2.*l6.*m8.*sin(th2 - th6) - dth6.^2.*l2.*l6.*m9.*sin(th2 - th6) - dth6.^2.*l2.*l6.*m10.*sin(th2 - th6) - dth7.^2.*l2.*l7.*m8.*sin(th2 - th7) - dth7.^2.*l2.*l7.*m9.*sin(th2 - th7) - dth7.^2.*l2.*l7.*m10.*sin(th2 - th7) - dth8.^2.*l2.*l8.*m9.*sin(th2 - th8) - dth8.^2.*l2.*l8.*m10.*sin(th2 - th8) - dth9.^2.*l2.*l9.*m10.*sin(th2 - th9);
f(3,:) = d3.*dth1.^2.*l1.*m3.*sin(th1 - th3) - g.*l3.*m4.*cos(th3) - g.*l3.*m5.*cos(th3) - g.*l3.*m6.*cos(th3) - g.*l3.*m7.*cos(th3) - g.*l3.*m8.*cos(th3) - g.*l3.*m9.*cos(th3) - g.*l3.*m10.*cos(th3) - d3.*g.*m3.*cos(th3) + d3.*dth2.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l3.*m5.*sin(th3 - th5) - d6.*dth6.^2.*l3.*m6.*sin(th3 - th6) - d7.*dth7.^2.*l3.*m7.*sin(th3 - th7) - d8.*dth8.^2.*l3.*m8.*sin(th3 - th8) - d9.*dth9.^2.*l3.*m9.*sin(th3 - th9) - d10.*dth10.^2.*l3.*m10.*sin(th3 - th10) + dth1.^2.*l1.*l3.*m4.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m5.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m6.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m7.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m4.*sin(th2 - th3) + dth1.^2.*l1.*l3.*m8.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m5.*sin(th2 - th3) + dth1.^2.*l1.*l3.*m9.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m6.*sin(th2 - th3) + dth1.^2.*l1.*l3.*m10.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m7.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m8.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m9.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m10.*sin(th2 - th3) - dth4.^2.*l3.*l4.*m5.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m6.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m7.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m8.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m9.*sin(th3 - th4) - dth5.^2.*l3.*l5.*m6.*sin(th3 - th5) - dth4.^2.*l3.*l4.*m10.*sin(th3 - th4) - dth5.^2.*l3.*l5.*m7.*sin(th3 - th5) - dth5.^2.*l3.*l5.*m8.*sin(th3 - th5) - dth5.^2.*l3.*l5.*m9.*sin(th3 - th5) - dth5.^2.*l3.*l5.*m10.*sin(th3 - th5) - dth6.^2.*l3.*l6.*m7.*sin(th3 - th6) - dth6.^2.*l3.*l6.*m8.*sin(th3 - th6) - dth6.^2.*l3.*l6.*m9.*sin(th3 - th6) - dth6.^2.*l3.*l6.*m10.*sin(th3 - th6) - dth7.^2.*l3.*l7.*m8.*sin(th3 - th7) - dth7.^2.*l3.*l7.*m9.*sin(th3 - th7) - dth7.^2.*l3.*l7.*m10.*sin(th3 - th7) - dth8.^2.*l3.*l8.*m9.*sin(th3 - th8) - dth8.^2.*l3.*l8.*m10.*sin(th3 - th8) - dth9.^2.*l3.*l9.*m10.*sin(th3 - th9);
f(4,:) = d4.*dth1.^2.*l1.*m4.*sin(th1 - th4) - g.*l4.*m5.*cos(th4) - g.*l4.*m6.*cos(th4) - g.*l4.*m7.*cos(th4) - g.*l4.*m8.*cos(th4) - g.*l4.*m9.*cos(th4) - g.*l4.*m10.*cos(th4) - d4.*g.*m4.*cos(th4) + d4.*dth2.^2.*l2.*m4.*sin(th2 - th4) + d4.*dth3.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l4.*m6.*sin(th4 - th6) - d7.*dth7.^2.*l4.*m7.*sin(th4 - th7) - d8.*dth8.^2.*l4.*m8.*sin(th4 - th8) - d9.*dth9.^2.*l4.*m9.*sin(th4 - th9) - d10.*dth10.^2.*l4.*m10.*sin(th4 - th10) + dth1.^2.*l1.*l4.*m5.*sin(th1 - th4) + dth1.^2.*l1.*l4.*m6.*sin(th1 - th4) + dth1.^2.*l1.*l4.*m7.*sin(th1 - th4) + dth1.^2.*l1.*l4.*m8.*sin(th1 - th4) + dth2.^2.*l2.*l4.*m5.*sin(th2 - th4) + dth1.^2.*l1.*l4.*m9.*sin(th1 - th4) + dth2.^2.*l2.*l4.*m6.*sin(th2 - th4) + dth1.^2.*l1.*l4.*m10.*sin(th1 - th4) + dth2.^2.*l2.*l4.*m7.*sin(th2 - th4) + dth2.^2.*l2.*l4.*m8.*sin(th2 - th4) + dth3.^2.*l3.*l4.*m5.*sin(th3 - th4) + dth2.^2.*l2.*l4.*m9.*sin(th2 - th4) + dth3.^2.*l3.*l4.*m6.*sin(th3 - th4) + dth2.^2.*l2.*l4.*m10.*sin(th2 - th4) + dth3.^2.*l3.*l4.*m7.*sin(th3 - th4) + dth3.^2.*l3.*l4.*m8.*sin(th3 - th4) + dth3.^2.*l3.*l4.*m9.*sin(th3 - th4) + dth3.^2.*l3.*l4.*m10.*sin(th3 - th4) - dth5.^2.*l4.*l5.*m6.*sin(th4 - th5) - dth5.^2.*l4.*l5.*m7.*sin(th4 - th5) - dth5.^2.*l4.*l5.*m8.*sin(th4 - th5) - dth5.^2.*l4.*l5.*m9.*sin(th4 - th5) - dth5.^2.*l4.*l5.*m10.*sin(th4 - th5) - dth6.^2.*l4.*l6.*m7.*sin(th4 - th6) - dth6.^2.*l4.*l6.*m8.*sin(th4 - th6) - dth6.^2.*l4.*l6.*m9.*sin(th4 - th6) - dth6.^2.*l4.*l6.*m10.*sin(th4 - th6) - dth7.^2.*l4.*l7.*m8.*sin(th4 - th7) - dth7.^2.*l4.*l7.*m9.*sin(th4 - th7) - dth7.^2.*l4.*l7.*m10.*sin(th4 - th7) - dth8.^2.*l4.*l8.*m9.*sin(th4 - th8) - dth8.^2.*l4.*l8.*m10.*sin(th4 - th8) - dth9.^2.*l4.*l9.*m10.*sin(th4 - th9);
f(5,:) = d5.*dth1.^2.*l1.*m5.*sin(th1 - th5) - g.*l5.*m6.*cos(th5) - g.*l5.*m7.*cos(th5) - g.*l5.*m8.*cos(th5) - g.*l5.*m9.*cos(th5) - g.*l5.*m10.*cos(th5) - d5.*g.*m5.*cos(th5) + d5.*dth2.^2.*l2.*m5.*sin(th2 - th5) + d5.*dth3.^2.*l3.*m5.*sin(th3 - th5) + d5.*dth4.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l5.*m6.*sin(th5 - th6) - d7.*dth7.^2.*l5.*m7.*sin(th5 - th7) - d8.*dth8.^2.*l5.*m8.*sin(th5 - th8) - d9.*dth9.^2.*l5.*m9.*sin(th5 - th9) - d10.*dth10.^2.*l5.*m10.*sin(th5 - th10) + dth1.^2.*l1.*l5.*m6.*sin(th1 - th5) + dth1.^2.*l1.*l5.*m7.*sin(th1 - th5) + dth1.^2.*l1.*l5.*m8.*sin(th1 - th5) + dth1.^2.*l1.*l5.*m9.*sin(th1 - th5) + dth2.^2.*l2.*l5.*m6.*sin(th2 - th5) + dth1.^2.*l1.*l5.*m10.*sin(th1 - th5) + dth2.^2.*l2.*l5.*m7.*sin(th2 - th5) + dth2.^2.*l2.*l5.*m8.*sin(th2 - th5) + dth2.^2.*l2.*l5.*m9.*sin(th2 - th5) + dth3.^2.*l3.*l5.*m6.*sin(th3 - th5) + dth2.^2.*l2.*l5.*m10.*sin(th2 - th5) + dth3.^2.*l3.*l5.*m7.*sin(th3 - th5) + dth3.^2.*l3.*l5.*m8.*sin(th3 - th5) + dth3.^2.*l3.*l5.*m9.*sin(th3 - th5) + dth4.^2.*l4.*l5.*m6.*sin(th4 - th5) + dth3.^2.*l3.*l5.*m10.*sin(th3 - th5) + dth4.^2.*l4.*l5.*m7.*sin(th4 - th5) + dth4.^2.*l4.*l5.*m8.*sin(th4 - th5) + dth4.^2.*l4.*l5.*m9.*sin(th4 - th5) + dth4.^2.*l4.*l5.*m10.*sin(th4 - th5) - dth6.^2.*l5.*l6.*m7.*sin(th5 - th6) - dth6.^2.*l5.*l6.*m8.*sin(th5 - th6) - dth6.^2.*l5.*l6.*m9.*sin(th5 - th6) - dth6.^2.*l5.*l6.*m10.*sin(th5 - th6) - dth7.^2.*l5.*l7.*m8.*sin(th5 - th7) - dth7.^2.*l5.*l7.*m9.*sin(th5 - th7) - dth7.^2.*l5.*l7.*m10.*sin(th5 - th7) - dth8.^2.*l5.*l8.*m9.*sin(th5 - th8) - dth8.^2.*l5.*l8.*m10.*sin(th5 - th8) - dth9.^2.*l5.*l9.*m10.*sin(th5 - th9);
f(6,:) = d6.*dth1.^2.*l1.*m6.*sin(th1 - th6) - g.*l6.*m7.*cos(th6) - g.*l6.*m8.*cos(th6) - g.*l6.*m9.*cos(th6) - g.*l6.*m10.*cos(th6) - d6.*g.*m6.*cos(th6) + d6.*dth2.^2.*l2.*m6.*sin(th2 - th6) + d6.*dth3.^2.*l3.*m6.*sin(th3 - th6) + d6.*dth4.^2.*l4.*m6.*sin(th4 - th6) + d6.*dth5.^2.*l5.*m6.*sin(th5 - th6) - d7.*dth7.^2.*l6.*m7.*sin(th6 - th7) - d8.*dth8.^2.*l6.*m8.*sin(th6 - th8) - d9.*dth9.^2.*l6.*m9.*sin(th6 - th9) - d10.*dth10.^2.*l6.*m10.*sin(th6 - th10) + dth1.^2.*l1.*l6.*m7.*sin(th1 - th6) + dth1.^2.*l1.*l6.*m8.*sin(th1 - th6) + dth1.^2.*l1.*l6.*m9.*sin(th1 - th6) + dth1.^2.*l1.*l6.*m10.*sin(th1 - th6) + dth2.^2.*l2.*l6.*m7.*sin(th2 - th6) + dth2.^2.*l2.*l6.*m8.*sin(th2 - th6) + dth2.^2.*l2.*l6.*m9.*sin(th2 - th6) + dth2.^2.*l2.*l6.*m10.*sin(th2 - th6) + dth3.^2.*l3.*l6.*m7.*sin(th3 - th6) + dth3.^2.*l3.*l6.*m8.*sin(th3 - th6) + dth3.^2.*l3.*l6.*m9.*sin(th3 - th6) + dth3.^2.*l3.*l6.*m10.*sin(th3 - th6) + dth4.^2.*l4.*l6.*m7.*sin(th4 - th6) + dth4.^2.*l4.*l6.*m8.*sin(th4 - th6) + dth4.^2.*l4.*l6.*m9.*sin(th4 - th6) + dth4.^2.*l4.*l6.*m10.*sin(th4 - th6) + dth5.^2.*l5.*l6.*m7.*sin(th5 - th6) + dth5.^2.*l5.*l6.*m8.*sin(th5 - th6) + dth5.^2.*l5.*l6.*m9.*sin(th5 - th6) + dth5.^2.*l5.*l6.*m10.*sin(th5 - th6) - dth7.^2.*l6.*l7.*m8.*sin(th6 - th7) - dth7.^2.*l6.*l7.*m9.*sin(th6 - th7) - dth7.^2.*l6.*l7.*m10.*sin(th6 - th7) - dth8.^2.*l6.*l8.*m9.*sin(th6 - th8) - dth8.^2.*l6.*l8.*m10.*sin(th6 - th8) - dth9.^2.*l6.*l9.*m10.*sin(th6 - th9);
f(7,:) = d7.*dth1.^2.*l1.*m7.*sin(th1 - th7) - g.*l7.*m8.*cos(th7) - g.*l7.*m9.*cos(th7) - g.*l7.*m10.*cos(th7) - d7.*g.*m7.*cos(th7) + d7.*dth2.^2.*l2.*m7.*sin(th2 - th7) + d7.*dth3.^2.*l3.*m7.*sin(th3 - th7) + d7.*dth4.^2.*l4.*m7.*sin(th4 - th7) + d7.*dth5.^2.*l5.*m7.*sin(th5 - th7) + d7.*dth6.^2.*l6.*m7.*sin(th6 - th7) - d8.*dth8.^2.*l7.*m8.*sin(th7 - th8) - d9.*dth9.^2.*l7.*m9.*sin(th7 - th9) - d10.*dth10.^2.*l7.*m10.*sin(th7 - th10) + dth1.^2.*l1.*l7.*m8.*sin(th1 - th7) + dth1.^2.*l1.*l7.*m9.*sin(th1 - th7) + dth1.^2.*l1.*l7.*m10.*sin(th1 - th7) + dth2.^2.*l2.*l7.*m8.*sin(th2 - th7) + dth2.^2.*l2.*l7.*m9.*sin(th2 - th7) + dth2.^2.*l2.*l7.*m10.*sin(th2 - th7) + dth3.^2.*l3.*l7.*m8.*sin(th3 - th7) + dth3.^2.*l3.*l7.*m9.*sin(th3 - th7) + dth3.^2.*l3.*l7.*m10.*sin(th3 - th7) + dth4.^2.*l4.*l7.*m8.*sin(th4 - th7) + dth4.^2.*l4.*l7.*m9.*sin(th4 - th7) + dth4.^2.*l4.*l7.*m10.*sin(th4 - th7) + dth5.^2.*l5.*l7.*m8.*sin(th5 - th7) + dth5.^2.*l5.*l7.*m9.*sin(th5 - th7) + dth5.^2.*l5.*l7.*m10.*sin(th5 - th7) + dth6.^2.*l6.*l7.*m8.*sin(th6 - th7) + dth6.^2.*l6.*l7.*m9.*sin(th6 - th7) + dth6.^2.*l6.*l7.*m10.*sin(th6 - th7) - dth8.^2.*l7.*l8.*m9.*sin(th7 - th8) - dth8.^2.*l7.*l8.*m10.*sin(th7 - th8) - dth9.^2.*l7.*l9.*m10.*sin(th7 - th9);
f(8,:) = d8.*dth1.^2.*l1.*m8.*sin(th1 - th8) - g.*l8.*m9.*cos(th8) - g.*l8.*m10.*cos(th8) - d8.*g.*m8.*cos(th8) + d8.*dth2.^2.*l2.*m8.*sin(th2 - th8) + d8.*dth3.^2.*l3.*m8.*sin(th3 - th8) + d8.*dth4.^2.*l4.*m8.*sin(th4 - th8) + d8.*dth5.^2.*l5.*m8.*sin(th5 - th8) + d8.*dth6.^2.*l6.*m8.*sin(th6 - th8) + d8.*dth7.^2.*l7.*m8.*sin(th7 - th8) - d9.*dth9.^2.*l8.*m9.*sin(th8 - th9) - d10.*dth10.^2.*l8.*m10.*sin(th8 - th10) + dth1.^2.*l1.*l8.*m9.*sin(th1 - th8) + dth1.^2.*l1.*l8.*m10.*sin(th1 - th8) + dth2.^2.*l2.*l8.*m9.*sin(th2 - th8) + dth2.^2.*l2.*l8.*m10.*sin(th2 - th8) + dth3.^2.*l3.*l8.*m9.*sin(th3 - th8) + dth3.^2.*l3.*l8.*m10.*sin(th3 - th8) + dth4.^2.*l4.*l8.*m9.*sin(th4 - th8) + dth4.^2.*l4.*l8.*m10.*sin(th4 - th8) + dth5.^2.*l5.*l8.*m9.*sin(th5 - th8) + dth5.^2.*l5.*l8.*m10.*sin(th5 - th8) + dth6.^2.*l6.*l8.*m9.*sin(th6 - th8) + dth6.^2.*l6.*l8.*m10.*sin(th6 - th8) + dth7.^2.*l7.*l8.*m9.*sin(th7 - th8) + dth7.^2.*l7.*l8.*m10.*sin(th7 - th8) - dth9.^2.*l8.*l9.*m10.*sin(th8 - th9);
f(9,:) = d9.*dth1.^2.*l1.*m9.*sin(th1 - th9) - g.*l9.*m10.*cos(th9) - d9.*g.*m9.*cos(th9) + d9.*dth2.^2.*l2.*m9.*sin(th2 - th9) + d9.*dth3.^2.*l3.*m9.*sin(th3 - th9) + d9.*dth4.^2.*l4.*m9.*sin(th4 - th9) + d9.*dth5.^2.*l5.*m9.*sin(th5 - th9) + d9.*dth6.^2.*l6.*m9.*sin(th6 - th9) + d9.*dth7.^2.*l7.*m9.*sin(th7 - th9) + d9.*dth8.^2.*l8.*m9.*sin(th8 - th9) - d10.*dth10.^2.*l9.*m10.*sin(th9 - th10) + dth1.^2.*l1.*l9.*m10.*sin(th1 - th9) + dth2.^2.*l2.*l9.*m10.*sin(th2 - th9) + dth3.^2.*l3.*l9.*m10.*sin(th3 - th9) + dth4.^2.*l4.*l9.*m10.*sin(th4 - th9) + dth5.^2.*l5.*l9.*m10.*sin(th5 - th9) + dth6.^2.*l6.*l9.*m10.*sin(th6 - th9) + dth7.^2.*l7.*l9.*m10.*sin(th7 - th9) + dth8.^2.*l8.*l9.*m10.*sin(th8 - th9);
f(10,:) = d10.*m10.*(dth1.^2.*l1.*sin(th1 - th10) - g.*cos(th10) + dth2.^2.*l2.*sin(th2 - th10) + dth3.^2.*l3.*sin(th3 - th10) + dth4.^2.*l4.*sin(th4 - th10) + dth5.^2.*l5.*sin(th5 - th10) + dth6.^2.*l6.*sin(th6 - th10) + dth7.^2.*l7.*sin(th7 - th10) + dth8.^2.*l8.*sin(th8 - th10) + dth9.^2.*l9.*sin(th9 - th10));

for i=1:nTime 
    MM = M(:,:,i);  ff = f(:,i); 
    dz(11:20,i) = -MM \ ff;
end 

end 
